#include "./Coordinate_Velocity.hpp"

void Coordinate_Class::Clear(void)
{
    x_coor = 0.0f;
    y_coor = 0.0f;
    angle_coor = 0.0f;
}

float Coordinate_Class::Trans_Rad(void) const
{
    return angle_coor / 180.0f * PI;
}

void Coordinate_Class::Angle_Trans(const float base_angle)
{
    angle_coor = Angle_Trans(angle_coor, base_angle);
}

float Coordinate_Class::Angle_Trans(float angle, const float base)
{
    int k = (int)((angle - base) / 360.0f);
    angle -= k * 360;
    if (angle - base > 180.0f)
    {
        angle -= 360.0f;
    }
    else if (angle - base <= -180.0f)
    {
        angle += 360.0f;
    }
    return angle;
}

Coordinate_Class &Coordinate_Class::operator+=(const Coordinate_Class &addend)
{
    Coordinate_Class temp;
    Coordinate_Class::Relative_To_Absolute(temp, addend, *this);
    *this = temp;
    return *this;
}

Coordinate_Class &Coordinate_Class::operator-=(const Coordinate_Class &subtrahend)
{
    Coordinate_Class temp;
    Coordinate_Class::Absolute_To_Relative(*this, temp, subtrahend);
    *this = temp;
    return *this;
}

Coordinate_Class &Coordinate_Class::Relative_To_Absolute(Coordinate_Class &Absolute_Coor, const Coordinate_Class &Relative_Coor, const Coordinate_Class &Base_Coor)
{
    const float angle = ((int)(Base_Coor.angle_coor * 1000.0f + 0.5f)) / 1000.0f;
    const float rad = Coordinate_Class::Trans_Rad(angle); /* 角度对应的弧度 */

    const float sin_Angle = arm_sin_f32(rad); /* 计算正弦值 */
    const float cos_Angle = arm_cos_f32(rad); /* 计算余弦值 */

    Absolute_Coor.x_coor = cos_Angle * (Relative_Coor.x_coor) - sin_Angle * (Relative_Coor.y_coor) + Base_Coor.x_coor;
    Absolute_Coor.y_coor = sin_Angle * (Relative_Coor.x_coor) + cos_Angle * (Relative_Coor.y_coor) + Base_Coor.y_coor;
    Absolute_Coor.angle_coor = Relative_Coor.angle_coor + Base_Coor.angle_coor;

    return Absolute_Coor;
}

Coordinate_Class &Coordinate_Class::Absolute_To_Relative(const Coordinate_Class &Absolute_Coor, Coordinate_Class &Relative_Coor, const Coordinate_Class &Base_Coor)
{
    const float angle = ((int)(Base_Coor.angle_coor * 1000.0f + 0.5f)) / 1000.0f;
    const float rad = Coordinate_Class::Trans_Rad(angle); /* 角度对应的弧度 */

    const float sin_Angle = arm_sin_f32(rad); /* 计算正弦值 */
    const float cos_Angle = arm_cos_f32(rad); /* 计算余弦值 */

    Relative_Coor.x_coor = cos_Angle * (Absolute_Coor.x_coor - Base_Coor.x_coor) + sin_Angle * (Absolute_Coor.y_coor - Base_Coor.y_coor);
    Relative_Coor.y_coor = (-sin_Angle) * (Absolute_Coor.x_coor - Base_Coor.x_coor) + cos_Angle * (Absolute_Coor.y_coor - Base_Coor.y_coor);
    Relative_Coor.angle_coor = Absolute_Coor.angle_coor - Base_Coor.angle_coor;

    return Relative_Coor;
}

//不符合交换律
Coordinate_Class operator+(const Coordinate_Class &summand, const Coordinate_Class &addend)
{
    Coordinate_Class temp = summand;
    temp += addend;
    return temp;
}

Coordinate_Class operator-(const Coordinate_Class &minuend, const Coordinate_Class &subtrahend)
{
    Coordinate_Class temp = minuend;
    temp -= subtrahend;
    return temp;
}

Coordinate_Class operator*(const Coordinate_Class &source, const float factor)
{
    Coordinate_Class temp;
    temp.x_coor = source.x_coor * factor;
    temp.y_coor = source.y_coor * factor;
    temp.angle_coor = source.angle_coor * factor;
    return temp;
}

void Velocity_Class::Clear(void)
{
    velocity_x = 0.0f;
    velocity_y = 0.0f;
    velocity_angle = 0.0f;
}

float Velocity_Class::Trans_Rad(void) const
{
    return velocity_angle / 180.0f * PI;
}

Velocity_Class &Velocity_Class::operator*=(const float factor)
{
    velocity_x *= factor;
    velocity_y *= factor;
    velocity_angle *= factor;
    return *this;
}

Velocity_Class &Velocity_Class::operator/=(const float divisor)
{
    this->operator*=(1.0f / divisor);
    return *this;
}

Velocity_Class &Velocity_Class::Relative_To_Absolute(Velocity_Class &Absolute_Velocity, const Velocity_Class &Relative_Velocity, const Coordinate_Class &Base_Coor)
{
    const float angle = ((int)(Base_Coor.angle_coor * 1000.0f + 0.5f)) / 1000.0f;
    const float rad = Coordinate_Class::Trans_Rad(angle); /* 角度对应的弧度 */

    const float sin_Angle = arm_sin_f32(rad); /* 计算正弦值 */
    const float cos_Angle = arm_cos_f32(rad); /* 计算余弦值 */

    Absolute_Velocity.velocity_x = cos_Angle * (Relative_Velocity.velocity_x) - sin_Angle * (Relative_Velocity.velocity_y);
    Absolute_Velocity.velocity_y = sin_Angle * (Relative_Velocity.velocity_x) + cos_Angle * (Relative_Velocity.velocity_y);
    Absolute_Velocity.velocity_angle = Relative_Velocity.velocity_angle;
    return Absolute_Velocity;
}

Velocity_Class &Velocity_Class::Absolute_To_Relative(const Velocity_Class &Absolute_Velocity, Velocity_Class &Relative_Velocity, const Coordinate_Class &Base_Coor)
{
    const float angle = ((int)(Base_Coor.angle_coor * 1000.0f + 0.5f)) / 1000.0f;
    const float rad = Coordinate_Class::Trans_Rad(angle); /* 角度对应的弧度 */

    const float sin_Angle = arm_sin_f32(rad); /* 计算正弦值 */
    const float cos_Angle = arm_cos_f32(rad); /* 计算余弦值 */

    Relative_Velocity.velocity_x = cos_Angle * (Absolute_Velocity.velocity_x) + sin_Angle * (Absolute_Velocity.velocity_y);
    Relative_Velocity.velocity_y = (-sin_Angle) * (Absolute_Velocity.velocity_x) + cos_Angle * (Absolute_Velocity.velocity_y);
    Relative_Velocity.velocity_angle = (Absolute_Velocity.velocity_angle);
    return Relative_Velocity;
}
